#!/usr/bin/env python

import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs.point_cloud2 import create_cloud_xyz32, create_cloud_xyz32

class LidarProcessor:
    def __init__(self):
        rospy.init_node('lidar_processor', anonymous=True)
        self.subscriber = rospy.Subscriber("/livox/lidar", PointCloud2, self.lidar_callback)
        self.publisher = rospy.Publisher("/livox/lidar_rotated", PointCloud2, queue_size=10)

    def lidar_callback(self, data):
        # 在这里进行旋转处理
        rotated_points = self.rotate_points(data)
        self.publisher.publish(rotated_points)

    def rotate_points(self, point_cloud):
        # 在这里实现绕x轴旋转180度的操作
        # 旋转操作可以通过对点云数据进行转换来实现
        # 这里假设点云数据是由(x, y, z)坐标组成的
        rotated_points = []  # 存储旋转后的点云数据
        for point in point_cloud:
            x, y, z = point
            # 绕x轴旋转180度
            rotated_y = -y
            rotated_z = -z
            rotated_points.append((x, rotated_y, rotated_z))
        return create_cloud_xyz32(point_cloud.header, rotated_points)

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    lidar_processor = LidarProcessor()
    lidar_processor.run()
